#pragma once
#include <omp.h>

template<class T>
void matrixMatrixMult(T *a, T *b, T *res, size_t n, size_t m, size_t k)
{
	for (size_t i = 0; i < n; i++)
	{
		for (size_t j = 0; j < k; j++)
		{
			//res[i][j] = 0.0;
			*(res + i*k + j) = 0.0;
			for (size_t l = 0; l < m; l++)
			{
				//res[i][j] += a[i][k]*b[k][j];
				*(res + i*k + j) += *(a + i * m + l) * *(b + l * k + j);
			}
		}
	}
}

template<class T>
void matrixAdd(T *res, T *a, size_t n, size_t m)
{
	for (size_t i = 0; i < n; i++)
		for (size_t j = 0; j < m; j++)
		{
			*(res + i*n + j) += *(a + i*n + j);
		}
}

template<class T>
void matrixVectorMult(T *a, T *v, T *res, size_t n, size_t m)
{
	for (size_t i = 0; i < n; i++)
	{
		res[i] = 0.0;
		for (size_t j = 0; j < m; j++)
		{
			res[i] += *(a + i * m + j)  *  *(v + j);
		}
	}
}

void matrixVectorMult(double *a, double *v, double *res, size_t n, size_t m)
{
#pragma omp parallel for
	for (int i = 0; i < n; i++)
	{
		printf(" %d/%d", omp_get_thread_num(), omp_get_num_threads());
		
		res[i] = 0.0;
		for (size_t j = 0; j < m; j++)
		{
			res[i] += *(a + i * m + j)  *  *(v + j);
		}
	}
}

template<class T>
void matrixScalarMult(T *a, size_t n, size_t m, T alpha)
{
	for (size_t i = 0; i < n; i++)
		for (size_t j = 0; j < m; j++)
		{
			*(a + i*n + j) *= alpha;
		}
}

template<class T>
void matrixEAdd(T *a, size_t n)
{
	for (size_t i = 0; i < n; i++)
	{
		*(a + i*n + i) += 1.0;
	}
}

/*
*Midpoint method is method Runge-Kutta second-order:
*x(0)
*x(n+1) = x(n)+h
*y(n+1) = y(n) + h*f(  x(n) + h/2,    y(n) + h/2*f(x(n), y(n))  )
*
*Differential equation:
*u'=Au   A - Rnxn
*f(x,y) = Ay
*
*Algorithm:
*y(n+1) = y(n) + h*f(  x(n) + h/2,    y(n) + h/2*f(x(n), y(n))  ) =
*= y(n) + h*A(y(n) + h/2*A*y(n)) = 
*= (E + h*A + h*h/2*A*A)y(n) = 
*= Zy(n)
*
*y(n+1) = Zy(n)
*/
template<class T>
void midpointMethod(T *a, size_t n, T h, int steps, T *y)
{
	T *z = new T[n * n];
	matrixMatrixMult(a, a, z, n, n, n);
	matrixScalarMult(z, n, n, h/2);
	matrixAdd(z, a, n, n);
	matrixScalarMult(z, n, n, h);
	matrixEAdd(z, n);

	for(int i = 0; i < steps; i++)
	{
		matrixVectorMult(z, y + i * n, y + (i + 1)*n, n, n);
	}

	delete [] z;
}

void midpointMethod(double *a, size_t n, double h, int steps, double *y)
{
	double *z = new double[n * n];
	matrixMatrixMult(a, a, z, n, n, n);
	matrixScalarMult(z, n, n, h/2);
	matrixAdd(z, a, n, n);
	matrixScalarMult(z, n, n, h);
	matrixEAdd(z, n);

	for(int i = 0; i < steps; i++)
	{
		matrixVectorMult(z, y + i * n, y + (i + 1)*n, n, n);
	}

	delete [] z;
}


template<class T>
bool compareArrays(T *a, T *b, size_t n)
{
	size_t i = 0;
	for (; i < n; i++)
	{
		if (a[i] != b[i])
			break;
	}
	return i == n;
}